#include "onewheel.h"

OneWheel::OneWheel()
{
}

void OneWheel::setParameters(QList<double> params)
{
    diameter = params[1];
    thetaMax = (params[0]*2*180.)/(M_PI*diameter);
}

void OneWheel::addMotor(int freq, double delay, double tau, double imprecision)
{
    motors.append(new ContinuousMotor(this, motors.size(), freq, delay, tau, imprecision));
}

void OneWheel::addSensor(int freq, double delay, double imprecision)
{
    sensors.append(new MotorStateSensor(this, sensors.size(), freq, delay, imprecision));
}

void OneWheel::checkValue(int id)
{
    double theta = motors[id]->getState();

    if(theta > thetaMax)
    {
        motors[id]->setState(thetaMax);
    }
    else if(theta < 0)
    {
        motors[id]->setState(0);
    }
}

QList<double> OneWheel::getPosition()
{
    double x = ((M_PI/180.)*motors[0]->getState()*diameter)/2;
    QList<double> result;
    result << x;
    return(result);
}

QList<double> OneWheel::getPositionNormee()
{
    double x = motors[0]->getState()/thetaMax;
    QList<double> result;
    result << x;
    return result;
}

double OneWheel::getMotorStateNormee(int id)
{
    return  motors[id]->getState()/thetaMax;
}


void OneWheel::setInState(QList<double> state){
    motors[0]->setState(state[0]);
}

